% test_force_control

clc
clear
close all
format compact
addpath('../animations', '../controllers', '../dynamics', '../environments', '../library');

x_dot_vec = [0:0.1:2.0];
    
for iter = 1:length(x_dot_vec)

x_dot = x_dot_vec(iter);
y = 1.0;

uniform_damping = 10e3;

first_guess = ((4+x_dot/2)*pi/8);

td_angle = fc_fixed_point_solve;

[time_passive, force_passive] = passive_force_profile;

[time_fc, force_fc, output_fc, J_fc, controller_fc] = run_force_control(time_passive, force_passive);

% J_opt
J_fc

% close all
% plot(time_passive, force_passive, 'k--', time_opt, force_opt, 'r-', time_fc, force_fc, 'b-')

close all
plot(time_passive, force_passive, 'k--', time_fc, force_fc, 'b-')

% dxout_opt = output_opt{3,2}(end,ForceControlModelClass.dxci) - x_dot
% yout_opt = output_opt{3,2}(end,ForceControlModelClass.yci) - y

dxout_fc = output_fc{3,2}(end,ForceControlModelClass.dxci);
yout_fc = output_fc{3,2}(end,ForceControlModelClass.yci);

dxout_fc_error = output_fc{3,2}(end,ForceControlModelClass.dxci) - x_dot
yout_fc_error = output_fc{3,2}(end,ForceControlModelClass.yci) - y

max_torque = controller_fc.max_torque

T_in = 0.5*ForceControlModelClass.m*x_dot^2
V_in = ForceControlModelClass.m*ForceControlModelClass.g*y

T_out = 0.5*ForceControlModelClass.m*dxout_fc^2
V_out = ForceControlModelClass.m*ForceControlModelClass.g*yout_fc

E_net = T_out + V_out - T_in - V_in

E_adjust = J_fc-E_net


E_vec(iter) = E_adjust;
end

save('Data\forcedata')